#pragma once
#include <string>
#include <memory>
#include <vector>
#include <map>
#include <shared_mutex>

#include "task/motion_al.h"
#include "zuc/nml_intf/zucpos2.h"
#include "errcode.h"
#include "axisgroup/fsm.h"
#include "kine/kine.h"
#include "rtdev/rtrobot.h"

class JAKADynIntf;
namespace axis {
class Axis;
}

namespace rtdev {
class RtRobot;
}

namespace axisgroup {
class RobotSetIntf;
}
namespace axisgroup {
const uint32_t DEFAULT_SERIAL_NUM = 3119999;
const size_t AXISGROUP_MAX_CNT = 3;
const size_t AXISGROUP_MAX_AXIS = 7;

const int ERR_AFTER_BOOT_TIMEOUT = 0;
const int SHUTDOWN_AFTER_BOOT_TIMEOUT = 1;
class AxisGroup
{
public:
    enum FsmErrorType
    {
        ERR_NONE = 0,
        ERR_SINGULARITY,  // 奇异
        ERR_JOINT_OUT,    // 关节超限
        ERR_CART_OUT,     // 末端超限
    };
    enum class FsmState : uint8_t
    {
        Booting,   //上电初始化
        Disabled,  //未使能
        Standby,   //有电但且使能，但未运动
        Moving,
        Stopping,
        ErrorStop
    };
    enum class MotionType : uint8_t
    {
        None,
        Free,
        Drag,
        Servo,
        Coord,
    };
    struct SafetyConfig
    {
        uint8_t reduce_sig_chn;  //缩减信号索引
        double reduce_tcp_vel;
        RobJointData<int> clsn_sensitivity_lv;  //参考工况下的碰撞灵敏度等级

        // 关节相关限制
        RobJointVal min_jpos;
        RobJointVal max_jpos;
        RobJointData<int> jpos_eps;
        RobJointData<double> max_joint_vel;
        RobJointData<double> max_joint_acc;
        RobJointData<double> max_joint_jerk;

        // 笛卡尔相关限制
        double max_tcp_vel;
    };
    struct Config
    {
        int action_after_boot_timeout;  //本体上电超时后的动作。0：进入errorstop状态；1：切断电源

        double rapid_scale = 1.0;  // 目前看来只是coord模式的倍率
        double net_feed_scale;     // 倍率
        double cyclictime_sec;     // 规划插补周期
        double servo_time_sec;     // 伺服周期
        SafetyConfig safety;
    };
    struct Command
    {
        bool enable = false;
        bool power = false;
        int home_joint = -1;      // 需要回零的关节
        int home_wait_count = 0;  // 回零后，由于位置变化，会导致前馈异常，因此需要延时
    };

public:
    AxisGroup(uint32_t serial_num = DEFAULT_SERIAL_NUM);
    virtual ~AxisGroup() = default;
    int boot_init(int id, std::shared_ptr<rtdev::RtRobot> rtrobot);
    int boot_init(int serial_num, bool is_rt);
    int reset();
    virtual int reset_kine_dyn();
    int shutdown();  // 切换关联轴到Errorstop状态，并解除轴关联，并通知轴组管理器删除该轴组
    inline bool is_alive() const { return alive_flg_; }
    bool is_sim() const { return sim_flg_; }
    void set_sim(bool sim, uint32_t serial_num);
    bool is_enabled() const;
    inline bool is_enabling() const { return cmd_.enable && (fsm_sta_ == FsmState::Disabled); }
    inline bool is_powered() const { return cmd_.power && !is_booting(); }
    inline bool is_powering() const { return cmd_.power && is_booting(); }
    inline bool is_booting() const { return fsm_sta_ == FsmState::Booting; }
    bool is_homing() const;
    bool is_homed() const;

    int link_joint(axis::Axis* joint);
    void reset_links();
    std::vector<uint32_t> get_joint_ids();
    inline size_t joint_num() const { return joints.size(); }
    inline uint8_t get_group_id() const { return group_id_; }
    virtual FsmState get_state();
    //模型信息获取
    int get_serial_num() const { return serial_num_; }
    AxisGroupPose get_cur_cmd_pos() const { return cmd_rst_; }
    void set_cycle_time(double mot_sec, double servo_sec);

public:  // usr cmd
    void do_enable(bool en);
    void do_power(bool en);
    void do_servo_home(int joint);  // 伺服回零指令

protected:
    // 运动状态机
    int fsm_state_disable(bool first);
    int fsm_state_standby(bool first);
    int fsm_state_moving(bool first);
    int fsm_state_stopping(bool first);
    int fsm_state_error_stop(bool first);
    void reset_joint_planner();
    virtual void init_fsm();
    std::unique_ptr<FsmItf> fsm_sta_booting_;
    std::unique_ptr<FsmItf> fsm_sta_disabled_;
    std::unique_ptr<FsmItf> fsm_sta_standby_;
    std::unique_ptr<FsmItf> fsm_sta_errorstop_;
    FsmState fsm_sta_, last_fsm_sta_;

protected:
    void servo_home_end_action();  // 回零结束

public:
    void update_fsm();
    void change_to_booting();
    void change_to_errstat(RobEvCode error_code, bool reset_enable);
    void change_fsm_stat(FsmState new_sta);
    static const char* run_state_string(FsmState state);

public:
    std::shared_ptr<kine::KineItf> kine() { return kine_; }                          // 此机器人运动学接口
    std::shared_ptr<::JAKADynIntf> dyna() { return dyna_; }                          // 此机器人的动力学接口
    std::shared_ptr<rtdev::RtRobot> RtRobot();                                       // 此机器人的实时层机器人接口
    axisgroup::AgConfig::RobotConfig get_robot_active_config() const;                // 此机器人生效的配置
    rtdev::RtRobot::RtRobotStatus& get_rtrobot_status() { return rtrobot_status_; }  // 此机器人在axisgroup中所存储的rtrobot状态
    std::shared_ptr<RobotSetIntf> get_set_intf() const { return set_intf_; }         // 此机器人的配置接口

protected:
    std::shared_ptr<kine::KineItf> kine_;
    std::shared_ptr<::JAKADynIntf> dyna_;

    uint32_t serial_num_;      // 序列号，真机由rtrobot状态更新时确定；
    uint32_t sim_serial_num_;  // 仿真时的序列号，由TASK决定
    uint8_t group_id_;         // 自动分配
    bool alive_flg_;           // 真机由轴是否在线确定
    bool sim_flg_;             // 是否处于仿真模式

    std::vector<axis::Axis*> joints;                     // 启动或机型变化时link
    std::shared_ptr<rtdev::RtRobot> rtrobot_;            // 关联的实时层机器人
    Command cmd_;                                        // 用户的部分指令
    std::shared_ptr<RobotSetIntf> set_intf_;             // 机器人配置设置接口
    rtdev::RtRobot::RtRobotStatus rtrobot_status_ = {};  // 关联的rtrobot状态，每个周期刷新

    // 给TASK的数据
    std::shared_mutex al_data_mtx_;
    mot::al::RobotALIntf::Status robot_status_fdb_ = {};  // 准备给TASK的数据，每个周期更新，TASK每个周期通过get接口刷新到TASK本地

public:
    Config config_;
    AxisGroupPose desired_cmd_ = {};   // 规划位置，力控等偏移功能在此上面叠加
    AxisGroupPose cmd_rst_ = {};       // 指令位置，最终发送给伺服
    AxisGroupPose last_cmd_rst_ = {};  // 上一周期指令位置
    AxisGroupPose feedback_ = {};      // 伺服反馈位置

public:
    uint32_t tool_id_ = 0;        // 工具坐标系
    uint32_t wobj_id_ = 0;        // 用户坐标系
    uint32_t base_frame_id_ = 0;  // 本体基座不是固定时使用的基座坐标

    friend class FsmDisable;
    friend class FsmBooting;
    friend class FsmStandby;
    friend class FsmMoving;
    friend class FsmStopping;
    friend class FsmErrorStop;

    friend class PFsmFree;
    friend class PFsmDrag;
    friend class PFsmServo;
    friend class PFsmIdle;
    friend class PFsmCoord;
    friend class AxisGroupMgr;
    friend class RobotSetIntf;
};

}  // namespace axisgroup